import rclpy
from rclpy.node import Node
import cv2
import cv_bridge
import numpy as np
from sensor_msgs.msg import CompressedImage

class ThresholdTester(Node):
    def __init__(self):
        super().__init__('threshold_tester')
        self.get_logger().info("Start threshold tester.")

        self.bridge = cv_bridge.CvBridge()

        self.image_sub = self.create_subscription(CompressedImage, '/image/compressed', self.image_callback, 10)
        self.mask_pub = self.create_publisher(CompressedImage, '/mask_image/compressed', 10)

    def image_callback(self, msg):
        self.get_logger().info("Image callback triggered.")
        try:
            np_arr = np.frombuffer(msg.data, np.uint8)
            image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"CvBridge Error: {e}")
            return

        height, width, _ = image.shape
        roi_height = int(height / 3)
        roi_bottom = height
        roi_top = roi_bottom - roi_height
        roi = image[roi_top:roi_bottom, :]

        blurred = cv2.GaussianBlur(roi, (5, 5), 0)
        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)

        lower_black = np.array([0, 0, 0])
        upper_black = np.array([180, 255, 50])
        mask = cv2.inRange(hsv, lower_black, upper_black)

        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.erode(mask, kernel, iterations=1)
        mask = cv2.dilate(mask, kernel, iterations=1)

        # 发布mask图像
        mask_image = CompressedImage()
        mask_image.header.stamp = self.get_clock().now().to_msg()
        mask_image.format = "jpeg"
        mask_image.data = np.array(cv2.imencode('.jpg', mask)[1]).tobytes()
        self.mask_pub.publish(mask_image)

def main(args=None):
    rclpy.init(args=args)
    tester = ThresholdTester()
    rclpy.spin(tester)
    tester.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
